%% 
% 该代码根据前面已实现功能的EKFDSE改编
% 希望写成一个函数，为后面的数据比较做准备
% mpc
% f0
% Xd
% R
% H
% D
% deltt
% t_SW
% t_FC
% t_max
% t_response
% T
% Z_measure
function [X_est,Z_est,RMSE,n,s] = DSE_EKF(mpc,f0,Xd,R,H,D,deltt,t_SW,t_FC,t_max,t_response,T,Z_measure)
    Y=Ybus_new(mpc);                 % 计算节点导纳矩阵
    result=runpf(mpc);               % 求解该系统潮流
    w_syn=2*pi*f0; % 同步转速
    D=D/w_syn; % 阻尼系数/同步转速
    M=2*H/w_syn; % 文章中表达方法是w0/2H
    t_SW = t_SW + t_response;   % 系统检测到故障离故障开始多久
    t_FC = t_FC + t_response;   % 系统检测到故障结束离故障结束多久
    % 还是以潮流计算得到的稳态情况为初始值
    Vmag=result.bus(:, 8);              % Pu voltage magnitude of each buses 
    Vph=result.bus(:, 9);               % angle in degree
    V=Vmag.*exp(1j*Vph*pi/180);         % 系统电压
    P_jQ=conj(V).*(Y*V);                % Net Power at each node 网络各节点潮流
    S=conj(P_jQ);                       % 网络节点潮流
    S=S/mpc.baseMVA;                            % 标幺
    Sg=result.gen(:, 2)+1j*result.gen(:, 3);    % 发电机输出功率
    Sg=Sg/mpc.baseMVA;                          % 标幺
    gen_bus=result.gen(:, 1);  

    %% 下面进入暂态分析
    %% calculate Y22
    Y22=diag(1./(1j*Xd));                   % 电机阻抗理想参数
    SL=result.bus(:, 3)+1j*result.bus(:, 4);    % 计算结果中节点功率
    SL=SL/mpc.baseMVA;                                  % 标幺
    YL=conj(SL)./(abs(V).^2);                   % 求节点负荷等效导纳
    Y11=Y+diag(YL);                             % 线路自导纳矩阵，导纳矩阵加上负荷
    Y11(gen_bus, gen_bus)=Y11(gen_bus, gen_bus)+Y22;    % 在自导纳上加上发电机的导纳
    Y12=zeros(length(result.bus(:,1)), length(result.gen(:,1)));    % 节点数×发电机数
    for i=1:length(result.bus(:,1))                 % 遍历bus
        for k=1:length(result.gen(:,1))             % 遍历gen
            q=result.gen(k,1);                      % 储存q为gen所在的节点编号
            if i==q                                 % 对所有gen在的节点
                Y12(q,k)=-1/(R(k)+Xd(k)*1j);        % 计算Y12为发电机的导纳
            end
        end
    end
    Y21=transpose(Y12);                             % 转置
    Ybf=Y22-Y21*inv(Y11)*Y12 ;                      % 故障前的简化矩阵Ybf=Y22-Y21*Y11逆*Y12
    RV(:, :, 1)=-inv(Y11)*Y12;% 节点重构矩阵

    %% 故障点位置（认为运行过程中节点4接地，切除节点4到节点14的线路后故障消除）
    %% ！！！！！！！！！这里注意，一般运行状态下不会马上得知故障点、故障类型，于是需要一点诊断或预测步骤得到故障点故障类型！！！！！！！！！！！
    f11=5;      % 运行过程中节点4接地
    F=[5 6];   % 故障结束后节点4和节点14间的线路断开
    f1=F(1);
    f2=F(2);

    %% during fault
    Y11df=Y11;                                      % 导纳矩阵加负荷
    Y11df(f11, :)=[];                               % 对应行去掉
    Y11df(:,f11)=[];                                % 对应列去掉
    Y12df=Y12;                                      % Y12计算
    Y12df(f11, :)=[];                               % 对应行去掉
    Y21df=transpose(Y12df);                         % 取转置
    Ydf=Y22-Y21df*inv(Y11df)*Y12df;                 % 故障过程系统简化等效导纳矩阵
    RV(:, :, 2)=zeros(size(RV(:, :, 1)));
    RV(1:end-1, :, 2)=RV(1:end-1, :, 2)-inv(Y11df)*Y12df;

    %% after fault
    Y11after=Y11;                       % 故障后的Y11中的元素置零
    Y11after(f1,f2)=0;
    Y11after(f2,f1)=0;
    for i=1:length(result.branch(:,1))  % 遍历所有支路
        if (f1==result.branch(i,1)&& f2==result.branch(i,2))||(f2==result.branch(i,1)&& f1==result.branch(i,2))     % 支路的始末端为故障的始末端
            Y11after(f1,f1)=Y11after(f1,f1)-result.branch(i,5)*1j/2-1/(result.branch(i,3)+result.branch(i,4)*1j);   
            Y11after(f2,f2)=Y11after(f2,f2)-result.branch(i,5)*1j/2-1/(result.branch(i,3)+result.branch(i,4)*1j);   % 导纳矩阵重新计算（支路断开的计算方法）
        end
    end
    Yaf=Y22-Y21*inv(Y11after)*Y12 ;                 % 故障后简化导纳矩阵
    RV(:, :, 3)=-inv(Y11after)*Y12; 

    %% Initialization 初始化系统
    Ig=conj(Sg./V(1:length(result.gen(:, 1)))); % 发电机电流
    E0=V(gen_bus)+Ig.*(R+1j*Xd);                % 等效理想电源机端电压
    E_abs=abs(E0);
    I0=Ybf*E0;                                  % 理想电源输出电流
    delta0=angle(E0)*180/pi;                    % 没有用到  % 发电机初始电压相角  
    w0=zeros(length(Xd), 1);                    % w0都设为0
    X_0=[angle(E0); w0];                        % X_0为初始电压相角和w0的集合，w0为转速，初值设为0

    % Initialize power injection
    PG0=real(E0.*conj(I0));         % 理想发电机有功输入
    PM=PG0;                         % 代替了PG0不知道为什么要多这一步，PG0后面都没用到
    QG0=imag(E0.*conj(I0));         % 理想发电机无功输入

    YBUS(:, :, 1)=Ybf;              % 定义YBUS
    YBUS(:, :, 2)=Ydf; 
    YBUS(:, :, 3)=Yaf;              % 给三维数组分别赋故障前中后三个时间的导纳矩阵
    n=length(gen_bus);              % n个发电机节点
    s=length(result.bus(:, 1));     % s个节点
    ns=2*n;             % 2n个状态量
    nm=2*n+2*s;         % 2n+2s个测量量 n个发电机的有功无功 n个节点的电压幅值相角

    % Covariance Matrix
    sig=1e-2;         % 量测方差
    P=sig^2*eye(ns);  % Error covariance matrix 协方差矩阵初始值
    Q=sig^2*0.1*eye(ns);  % system noise covariance matrix 系统模型误差
    R=sig^2*0.1*eye(nm);  % measurment noise covariance matrix 测量噪声方差

    cnt = 0;    % 用于计数实现按不同频率调用EKF算法求解状态
    X_hat=X_0;  % 估计值初始化，相角为E0（）
    X_est=[];   % 估计值
    X_mes=[];   % Initial statel 
    Z_est=[];
    zhat=[];
    RMSE=[]; % 对比标准
    
    % Extended Kalman Filter (EKF) ALgorithm 使用扩展卡尔曼滤波算法求解
    for k=0:deltt:t_max     % 仿真出从0到结束的模型
        cnt = cnt+1;
        % Ybus and reconstruction matrix accodring to the requirement 给ps赋值以定下该时刻系统的运行状态
        if k<t_SW
            ps=1;
        elseif (t_SW<k)&&(k<=t_FC)
            ps=2;
        else 
            ps=3;
        end

        Ybusm = YBUS(:,:,ps);           % 取出对应状态下的Ybus
        RVm=RV(:, :, ps);               % 对应状态下的RV
        [~, X] = ode45(@(t,x) dynamic_system(t,x,M,D,Ybusm,E_abs,PM,n),[k k+deltt],X_0);        % 用ode45求解器求解理想系统动态过程
        X_0=transpose(X(end, :));       % 对X最后一行做转置，得到一列，X是上面求解器得到的结果，为理想状态下的状态值    
        if mod(cnt,T)==0              % 每500ms一次的计算
            z = Z_measure(:,cnt);       % 量测量对应的理想系统真值，格式：[PG; QG; Vmag; Vangle]
            % determine Phi=df/fx 预测步
            Phi=RK4partial(E_abs, X_hat, Ybusm, M, deltt*T, D, n);        % Phi是状态转移矩阵，里面出现偏导数是因为非线性问题线性化
            X_hat=RK4(n, deltt*T, E_abs, ns, X_hat, PM, M, D, Ybusm);     % 算X的估计值
            P=Phi*P*transpose(Phi)+Q;
            % correction 修正步
            [H, zhat]=RK4H(E_abs, X_hat, Ybusm, s,n, RVm,zhat) ;     % 得到偏导数H和由xhat得到的量测计算值
            % Measurement update of state estimate and estimation error covariance 
            K=P*transpose(H)*(H*P*transpose(H)+R);  % 卡尔曼增益
            X_hat=X_hat+ K*(z-zhat);                 % X的最优估计值，这里的z我认为有问题，应该加入一些量测噪声
            P=(eye(ns)-K*H)*P;                      % 认为预测步协方差矩阵为I，更新状态估计协方差矩阵
            X_est=[X_est, X_hat];
            Z_est=[Z_est, zhat];
            RMSE=[RMSE, sqrt(trace(P))];
        end
    end
    save('39_RMSE_EKF.mat', 'RMSE')
end

%% func
function Phi=RK4partial(E_abs, X_hat, Ybusm, M, deltt, D, n)
    
    % determine k1 for delta and omega of the dynamic equation dynamics 求解动态方程中的delta和omega需要使用的k1
    E1=E_abs.*exp(1j*X_hat(1:n));     
    I1=Ybusm*E1; 
    dE1=[diag(1j*E_abs.*exp(1j*X_hat(1:n))), zeros(n)];
    dI1=Ybusm*dE1; 
    dPG1=real(conj(diag(I1))*dE1+diag(E1)*conj(dI1)); 
    %k1_w
    d_k1w=(-1*deltt*diag(M.^-1))*dPG1+[zeros(n), (-1*deltt)*diag(D).*diag(M.^-1)];    
    d_w=[eye(n), zeros(n)]+d_k1w;
    
    %k1delta
    k1_delta=[zeros(n), deltt*eye(n)];
    d_delta=[zeros(n), eye(n)]+k1_delta; 
    Phi=[ d_delta; d_w];

end 

function [H, zhat]=RK4H(E_abs, X_hat, Ybusm, s,n, RVm,zhat) 
    % calculate zhat
    E1=E_abs.*exp(1j*X_hat(1:n));     
    I1=Ybusm*E1; 
    PG1=real(conj(I1).*E1); 
    QG1=imag(conj(I1).*E1);
    Vmag=abs(RVm*E1); 
    Vangle=angle(RVm*E1); 
    if ~isempty(zhat)
        Vangle = fix((zhat(2*n+s+1:2*n+2*s)-Vangle+pi)/(2*pi))*2*pi+Vangle;
    end
    zhat=[PG1; QG1; Vmag; Vangle]; 
    
    % calculate H
    dE1=[diag(1j*E_abs.*exp(1j*X_hat(1:n))), zeros(n)];
    dI1=Ybusm*dE1;    
    dPG1=real(conj(diag(I1))*dE1+diag(E1)*conj(dI1));
    dQG1=imag(conj(diag(I1))*dE1+diag(E1)*conj(dI1));
    dVmag=zeros(s, 2*n); 
    dVangle=[ones(s, n), zeros(s, n)];
    H=[dPG1; dQG1; dVmag; dVangle]; 
end 


%  USE ode45 to solve this differential equation 
function dx = dynamic_system(~,x,M,D, Ybusm,Vo,Pm,NumG)  % dynamics of the sin control system
Vg = Vo.*exp(1j*x(1:NumG));
Ibus = Ybusm*Vg;
S = conj(Ibus).*Vg;
Pe = real(S);
dx = zeros(2*NumG,1);
dx(1:NumG) = x(NumG+1:2*NumG);
dx(NumG+1:2*NumG) = (Pm-Pe)./M-D.*x(NumG+1:2*NumG)./M;
end 


function xbreve=RK4(n, deltt, E_abs, ns, X_hat, PM, M, D, Ybusm)
    % update sigma point 
    E1=E_abs.*exp(1j*X_hat(1:n, :)); 
    I1=Ybusm*E1; 
    PG1=real(E1.*conj(I1)); 
    % PM_rep=repmat(PM, 1, 2*ns);
    % M_rep=repmat(M, 1, 2*ns);
    % D_rep=repmat(D, 1, 2*ns); 
    
    % k1
    k1_w=deltt*(M.^-1.*(PM-PG1-(D.*(X_hat(n+1:ns, :)))));
    k1_delta=deltt*X_hat(n+1:ns, :);
    
    % k2
    E2=E_abs.*exp(1j*(X_hat(1:n, :)+k1_delta/2)); 
    I2=Ybusm*E2; 
    PG2=real(E2.*conj(I2));     
    k2_w=deltt*(M.^-1.*(PM-PG2-(D.*((X_hat(n+1:ns, :)+k1_w/2)))));
    k2_delta=deltt*(X_hat(n+1:ns, :)+k1_w/2); 
    
    % k3
    E3=E_abs.*exp(1j*(X_hat(1:n, :)+k2_delta/2)); 
    I3=Ybusm*E3; 
    PG3=real(E3.*conj(I3));     
    k3_w=deltt*(M.^-1.*(PM-PG3-(D.*((X_hat(n+1:ns, :)+k2_w/2)))));
    k3_delta=deltt*(X_hat(n+1:ns, :)+k2_w/2); 
    
    % k2
    E4=E_abs.*exp(1j*(X_hat(1:n, :)+k3_delta)); 
    I4=Ybusm*E4; 
    PG4=real(E4.*conj(I4));     
    k4_w=deltt*(M.^-1.*(PM-PG4-(D.*((X_hat(n+1:ns, :)+k3_w)))));
    k4_delta=deltt*(X_hat(n+1:ns, :)+k3_w); 
    
    xbreve(1:n, :)=X_hat(1:n, :)+ (k1_delta+2*k2_delta+2*k3_delta+k4_delta)/6;
    xbreve(n+1:ns, :)=X_hat(n+1:ns, :) + (k1_w+2*k2_w+2*k3_w+k4_w)/6;
end


